#include <assert.h>

#include <ros/ros.h>
#include <tf/transform_listener.h>

#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseArray.h>

#include <sensor_msgs/PointCloud2.h>

#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>

#include <opencv2/highgui/highgui.hpp>

#include "../scan_object/COM.h"
#include "scan_object/AddFrame.h"

using namespace std;
using namespace cv;
using namespace pcl;

COM* com;
Mat img, depth;
// TODO ne pas recreer un nouveau tf listener a chaque appel de camera2target
/*
3 sources d'erreur : 
    Utiliser camera/depth_registered/points
             et non
             camera/depth/points
    Calibrer correctement la camera rgb
    Verifier que openni_launch recupere les bon parametres intrinseques pour
    sa rectification.
*/

Mat camera2target (string camera, string target);

void cloud_callback(const sensor_msgs::PointCloud2& msg) {
	PointCloud<PointXYZRGB>::Ptr cloud (new PointCloud<PointXYZRGB>);
	fromROSMsg (msg, *cloud);
	unsigned int h = cloud->height;
	unsigned int w = cloud->width;
	Mat tmp_depth (h, w, CV_32FC3);
	Mat tmp_rgb (h, w, CV_8UC3);
	for (size_t y=0; y<h; ++y) {
		for (size_t x=0; x<w; ++x) {
			PointXYZRGB pt;
			pt = cloud->at(x,y);
			tmp_depth.at<Vec3f>(y, x)[0] = pt.x;
			tmp_depth.at<Vec3f>(y, x)[1] = pt.y;
			tmp_depth.at<Vec3f>(y, x)[2] = pt.z;
			int rgb = *reinterpret_cast<int*>(&pt.rgb);
            tmp_rgb.at<Vec3b>(y, x)[0] = (rgb & 0xff); // B
            tmp_rgb.at<Vec3b>(y, x)[1] = ((rgb >> 8) & 0xff); // G
            tmp_rgb.at<Vec3b>(y, x)[2] = ((rgb >> 16) & 0xff); // R
		}
	}
	depth = tmp_depth;
	img = tmp_rgb;
	//imshow ("depth", depth);
	//imshow ("rgb", tmp_rgb);
	//waitKey(0);
}

bool addFrame (scan_object::AddFrame::Request &req, 
                scan_object::AddFrame::Response &res) {
    ROS_INFO("Adding new frame");
	
    Mat P = camera2target (req.camera, req.target);
    com->addFrame (img, depth, P);
    
    return true;
}

// roslaunch openni_launch openni.launch camera:=kinect
// rosrun scan_object com /camera/depth_registered/points
// rosservice call AddFrame head_mount_kinect_ir_optical_frame r_gripper_tool_frame
int main(int argc, char** argv) {
	assert (argc == 2 && "Usage : com in_color_cloud_topic");
	
	ros::init(argc, argv, "cheating_object_modeler");
	ros::NodeHandle n;

    com = new COM;

	ros::Subscriber cloud_subscriber = n.subscribe(argv[1], 1, cloud_callback);
    ros::ServiceServer add_frame_service = n.advertiseService("AddFrame", addFrame);

    ros::Rate loop_rate(10);
    pcl::visualization::CloudViewer viewer ("Model");
    while (ros::ok() && !viewer.wasStopped ()) {
        ros::spinOnce();
        
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
        vec2pcd (com->p3d(), cloud);
        if (!cloud->points.empty())
            viewer.showCloud (cloud);
            
        loop_rate.sleep();
    }
            
    return 0;
}

Mat camera2target (string camera, string target) {
    tf::TransformListener listener;

	tf::StampedTransform transform;
    try {
        listener.waitForTransform(camera, target, ros::Time(0), ros::Duration(10.0) );
        listener.lookupTransform(camera, target, ros::Time(0), transform);
    }
    catch (tf::TransformException ex) {
        ROS_ERROR("%s",ex.what());
    }
    return transform2mat (transform);
}



